# -*- coding: utf-8 -*-
# @Time    : 2024/11/19 11:42
# @Author  : sjh
# @Site    : 
# @File    : genaral.py
# @Comment :
import os
import numpy as np
import ctypes
from scipy import signal
import cv2
import matplotlib.pyplot as plt

from matplotlib import rcParams
from scipy.signal import savgol_filter

# 设置字体为 SimHei (黑体)，适用于 Windows 系统
rcParams['font.sans-serif'] = ['SimHei']
rcParams['axes.unicode_minus'] = False  # 防止负号显示问题
def plot_peak(test_toe_list, part="right", highlight_points=None):
    L_start_time = [0]
    # 创建一个新的图形
    fig, axs = plt.subplots(3, 1, figsize=(10, 8))


    # 将数据拆分为 x, y, z 列表
    x_data = [point[0] for point in test_toe_list]
    y_data = [point[1] for point in test_toe_list]
    z_data = [point[2] for point in test_toe_list]
    time_data = list(range(len(test_toe_list)))  # 假设时间数据为索引

    # 绘制 X 变化曲线
    axs[0].plot(time_data, x_data, label='X', color='r')
    axs[0].set_xlabel('Time')
    axs[0].set_ylabel('X Value ' + part)
    axs[0].set_title('X变化曲线')
    axs[0].legend()

    # 绘制 Y 变化曲线
    axs[1].plot(time_data, y_data, label='Y', color='g')
    axs[1].set_xlabel('Time')
    axs[1].set_ylabel('Y Value ' + part)
    axs[1].set_title('Y变化曲线')
    axs[1].legend()

    # 绘制 Z 变化曲线
    axs[2].plot(time_data, z_data, label='Z', color='b')
    axs[2].set_xlabel('Time')
    axs[2].set_ylabel('Z Value ' + part)
    axs[2].set_title('Z变化曲线')
    axs[2].legend()

    # 如果 highlight_points 不为 None，则绘制高亮点
    if highlight_points is not None and highlight_points is not []:
        highlight_points = np.array(highlight_points)  # 确保是 NumPy 数组
        # 标记 X 曲线的高亮点
        axs[0].scatter(highlight_points,
                       np.array(x_data)[highlight_points], color='k', marker='o', label='Highlighted Points')
        axs[0].legend()
        # 标记 Y 曲线的高亮点
        axs[1].scatter(highlight_points,
                       np.array(y_data)[highlight_points], color='k', marker='o', label='Highlighted Points')
        axs[1].legend()
        # 标记 Z 曲线的高亮点
        axs[2].scatter(highlight_points,
                       np.array(z_data)[highlight_points], color='k', marker='o', label='Highlighted Points')
        axs[2].legend()

    # 调整子图间距
    plt.tight_layout()
    # 显示图形
    plt.show()
def plot_peak_1d(left_big_toe_list, part="right", highlight_points=None):
    import matplotlib.pyplot as plt
    fig, axs = plt.subplots(1, 1, figsize=(10, 4))
    test_toe_list = left_big_toe_list

    time_data = list(range(len(test_toe_list)))  # 假设时间数据为索引

    # 绘制 X 变化曲线
    axs.plot(time_data, test_toe_list, label='X', color='r')
    axs.set_xlabel('Time')
    axs.set_ylabel('X Value ' + part)
    axs.set_title('X变化曲线')
    axs.legend()

    # 如果 highlight_points 不为 None，则绘制高亮点
    if highlight_points:
        highlight_points = np.array(highlight_points)  # 确保是 NumPy 数组
        # 标记 X 曲线的高亮点
        axs.scatter(highlight_points,
                       np.array(test_toe_list)[highlight_points], color='k', marker='o', label='Highlighted Points')
        axs.legend()

    plt.tight_layout()
    plt.show()

def butterworth_filter_1d(col, args=(4,2,10)):
    '''
    1D Zero-phase Butterworth filter (dual pass)
    Deals with nans

    INPUT:
    - col: numpy array
    - order: int
    - cutoff: int
    - framerate: int

    OUTPUT
    - col_filtered: Filtered numpy array
    '''
    if not isinstance(col, np.ndarray):
        col = np.array(col)
    order, cutoff, framerate = args

    b, a = signal.butter(order / 2, cutoff / (framerate / 2), 'low', analog=False)
    padlen = 3 * max(len(a), len(b))

    # split into sequences of not nans
    col_filtered = col.copy()
    mask = np.isnan(col_filtered) | (col_filtered == 0)
    falsemask_indices = np.where(~mask)[0]
    gaps = np.where(np.diff(falsemask_indices) > 1)[0] + 1
    idx_sequences = np.split(falsemask_indices, gaps)
    if idx_sequences[0].size > 0:
        idx_sequences_to_filter = [seq for seq in idx_sequences if len(seq) > padlen]

        # Filter each of the selected sequences
        for seq_f in idx_sequences_to_filter:
            col_filtered[seq_f] = signal.filtfilt(b, a, col_filtered[seq_f])

    return col_filtered


def smooth_filter_1d(data, window_size=5):
    """
    使用滑动窗口对数据进行平滑。 Moving Average Filter

    Args:
        data (numpy.ndarray): 输入的差值数据。
        window_size (int): 滑动窗口的大小，必须为奇数。

    Returns:
        numpy.ndarray: 平滑后的数据。
    """
    if window_size % 2 == 0:
        raise ValueError("window_size 必须是奇数")

    # 使用卷积进行平滑处理
    smoothed_data = np.convolve(data, np.ones(window_size) / window_size, mode='valid')

    # 保持原始数据长度
    pad_size = (len(data) - len(smoothed_data)) // 2
    smoothed_data = np.pad(smoothed_data, (pad_size, pad_size), mode='edge')

    return smoothed_data
def savgol_filter_3d(data, window_length=10, polyorder=4):
    """
    对3D关键点数据进行Savitzky-Golay滤波。

    :param data: 输入的3D关键点数据，形状为 (帧数, 3)
    :param window_length: 滑动窗口大小，必须为奇数
    :param polyorder: 多项式阶数
    :return: 滤波后的3D关键点数据，形状为 (帧数, 3)
    """


    filtered_data = np.zeros_like(data)

    # 对每个关节点的 x, y, z 坐标分别进行滤波
    for dim in range(3):  # 遍历 x, y, z 三个维度
        filtered_data[:, dim] = savgol_filter(data[:, dim], window_length, polyorder)
    return filtered_data

def multi_frame_check_with_indices(data, frame_window=2, threshold=0.1):
    """
    通过多帧内的平均变化判断有效点，并将无效帧替换为初始帧数据，同时返回有效帧索引。

    Args:
        data (numpy.ndarray): 输入数据。
        frame_window (int): 判断窗口大小。
        threshold (float): 判断有效变化的阈值。

    Returns:
        tuple: (处理后的数据, 有效帧索引)
    """
    # 初始帧数据
    initial_frame = data[0]
    # 创建结果数组（以原始数据为基础）
    processed_data = data.copy()
    valid_indices = []

    for i in range(len(data)):
        # 如果是窗口范围内的帧，直接初始化为初始帧
        if i < frame_window:
            processed_data[i] = initial_frame
        else:
            # 计算滑动窗口的平均值
            window_mean = np.mean(data[i - frame_window:i])
            if np.abs(window_mean) > threshold:
                valid_indices.append(i)
            else:
                processed_data[i] = initial_frame

    return processed_data